#include "ros/ros.h"
#include "turtle_move/move.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include <math.h>

#define PI 3.1415926535

double old_x, old_y, old_angle;
double new_x, new_y, new_angle;

void getOLdPose(const turtlesim::Pose::ConstPtr &p)
{
    old_x = p->x;
    old_y = p->y;
    old_angle = p->theta;
}

void getNewPose(const turtlesim::Pose::ConstPtr &p)
{
    new_x = p->x;
    new_y = p->y;
    new_angle = p->theta;
}

bool doReq(turtle_move::move::Request &req,   turtle_move::move::Response &resp)
{
    ros::NodeHandle nh;
    ros::Rate r(1);

    ros::Subscriber sub1 = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1,getOLdPose);
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist> ("/turtle1/cmd_vel",1000);
    ros::Subscriber sub2 = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1,getNewPose);

    r.sleep();
    ros::spinOnce();
    ROS_INFO("当前坐标:(%.2lf, %.2lf),当前弧度:%.2lf", new_x, new_y, new_angle);

    double tar_x = req.x;
    double tar_y = req.y;
    double mov_x = tar_x - old_x;
    double mov_y = tar_y - old_y;
    double target_angle = atan(mov_y / mov_x);

    if ((mov_x < 0 && mov_y < 0) || (mov_x < 0 && mov_y > 0))
        target_angle += PI;
    double move_angle =  target_angle - old_angle;
    if (move_angle > PI)
        move_angle -= 2 * PI;       
    else if (move_angle < -PI)
        move_angle += 2 * PI;
    ROS_INFO("目标坐标:(%.2lf, %.2lf),目标弧度:%.2lf", tar_x, tar_y, target_angle);


    geometry_msgs::Twist msg;
    msg.linear.x = 0.0;
    msg.linear.y = 0.0;
    msg.linear.z = 0.0;
    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = move_angle;
    pub.publish(msg);
    ROS_INFO("即将开始移动......");
    r.sleep();

    msg.linear.x = sqrt(pow(mov_x, 2) + pow(mov_y, 2));
    msg.angular.z = 0.0;
    pub.publish(msg);
    r.sleep();

    ros::spinOnce();
    ROS_INFO("运动结束,当前坐标:(%.2lf, %.2lf),当前弧度:%.2lf", new_x, new_y, new_angle);

    resp.x = new_x;
    resp.y = new_y;
    resp.theta = new_angle;
    return true;
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");

    ros::init(argc, argv, "Move_Server");
    ros::NodeHandle nh;
    ros::ServiceServer server = nh.advertiseService("Move_Server", doReq);
    ros::spin();
    ROS_INFO("服务已经启动....");

    return 0;
}
